/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMMTools
* @author     Nikolaus Vahrenkamp
* @copyright  2013 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_MMMSimoxTools_H_
#define __MMM_MMMSimoxTools_H_

#include <Eigen/Core>
#include <string> 
#include <vector>
#include <map>

#include <MMM/MMMCore.h>
#include <MMM/MMMImportExport.h>
#include <MMM/Motion/Legacy/AbstractMotion.h>
#include <MMM/Motion/Legacy/MarkerMotion.h>
#include <MMM/Model/Model.h>

#include "MMMSimoxToolsImportExport.h"

#include <VirtualRobot/Robot.h>

namespace MMM
{

/*!
	\brief Several Simox related tools.
*/
namespace SimoxTools
{
	/*!
		Converts an MMM model to the Simox:::VirtualRobot format.
		The resulting robot can be used for visualizations and computations within the Simox framework.

		\param model The MMM model.
		\param loadVisualizations If set, the visualization files are loaded, otherwise the visualization buildup is skipped which results in faster loading.
		\return The VirtualRobot model.
	*/
    VirtualRobot::RobotPtr MMMSimoxTools_IMPORT_EXPORT buildModel(MMM::ModelPtr model, bool loadVisualizations = true);


	/*!
		Computes and overwrites teh inertial tensor matrices of all segments. 
		The bounding box of each segment's 3d model is determined in order to compute the standard inertia tensor for cubes.
		The resulting inertia matrices are generated without considering the mass. To get the mass-related inertia multiply the matrix with the robotnode's mass.
	*/
	bool MMMSimoxTools_IMPORT_EXPORT updateInertialMatricesFromModels(VirtualRobot::RobotPtr robot);


    VirtualRobot::RobotNodePtr MMMSimoxTools_IMPORT_EXPORT convertNode(MMM::ModelNodePtr modelNode, std::vector<MMM::ModelNodePtr> &MMMNodes, std::vector<VirtualRobot::RobotNodePtr>& allNodes, std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> > &childrenMap, VirtualRobot::RobotPtr robo, bool loadVisualizations);
	VirtualRobot::SensorPtr MMMSimoxTools_IMPORT_EXPORT convertMarker(MMM::MarkerInfoPtr m, VirtualRobot::RobotNodePtr rn, float scaling);

    /*!
        Builds a reduced Simox model by merging all robot nodes in subtrees below the given actuated joints of the model (e.g. hands of the MMM model).
    */
    VirtualRobot::RobotPtr MMMSimoxTools_IMPORT_EXPORT buildReducedModel(VirtualRobot::RobotPtr fullRobot, std::vector<std::string> actuatedJoints);
}


}
#endif 
